using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;
using System.Collections;

namespace Geometry
{
	/// <summary>
	/// PointCloud class is written for use with Raster.ReSampling namespace.
	/// </summary>
    public class PointCloud
    {
        private List<ColoredPoint>[,] pointCloud;

        /// <summary>
        /// Constructor method.
        /// </summary>
        /// <param name="rows">Number of rows</param>
        /// <param name="cols">Number of cols</param>
        public PointCloud(int rows, int cols)
        {
            pointCloud = new List<ColoredPoint>[rows, cols];
            for (int i = 0; i < rows; i++)
            {
                for (int j = 0; j < cols; j++)
                {
                    pointCloud[i,j] = new List<ColoredPoint>();
                }
            }
        }

        private int rowCount
        {
            get { return pointCloud.GetLength(0); }
        }

        private int colCount
        {
            get { return pointCloud.GetLength(1); }
        }

        /// <summary>Gets the number of rows.</summary>
        public int RowCount
        {
            get { return pointCloud.GetLength(0); }
        }

        /// <summary>Gets the number of columns.</summary>
        public int ColCount
        {
            get { return pointCloud.GetLength(1); }
        }

        /// <summary>
        /// Adds a ColoredPoint to the PointCloud grid.
        /// </summary>
        /// <param name="c">ColoredPoint object</param>
        public void Add(ColoredPoint c)
        {
            pointCloud[(int)c.Y, (int)c.X].Add(c);
        }

        /// <summary>
        /// This method is for doing interpolation in resampling.
        /// </summary>
        /// <remarks>
        /// Incomplete
        /// -Must check if size value make the loops throw a "Out of bounds" exception.
        /// </remarks>
        /// <param name="x">X offset of the interpolation point</param>
        /// <param name="y">Y offset of the interpolation point</param>
        /// <param name="count">Number of near to the interpolation point</param>
        /// <returns>
        /// The nearest "count" number of ColoredPoint objects
        /// </returns>
        public Color FindNear(int x, int y, int count)
        {
            int found = 0;
            int size = 1;
            int toX;
            int toY;
            int fromX;
            int fromY;
            double d;
            double[] dArr = new double[count + 1];
            ColoredPoint[] cArr = new ColoredPoint[count + 1];
            if ((x >= 0 && y >= 0) && (y < this.rowCount&& x < this.colCount ))
            {
                toY = (y + size > this.rowCount) ? this.rowCount : y + size - 1;
                fromY = (y - size < 0) ? 0 : y-size;
                toX = (x + size > this.colCount) ? this.colCount : x + size - 1;
                fromX = (x - size < 0) ? 0 : x-size;
                while (found < count)
                {
                    for (int i = fromY; i < toY; i++)
                    {
                        for (int j = fromX; j < toX; j++)
                        {
                            foreach (ColoredPoint c in pointCloud[i, j])
                            {
                                d = Math.Pow(c.X - x, 2) + Math.Pow(c.Y - y, 2);
                                if (found < count+1)
                                {
                                    dArr[count] = d;
                                    cArr[count] = c;
                                }
                                else
                                {
                                    dArr[0] = d;
                                    cArr[0] = c;
                                }
                                Array.Sort(dArr, cArr);
                                Array.Reverse(dArr);
                                Array.Reverse(cArr);
                                //Console.WriteLine(dArr[0]);
                                found++;
                            }
                        }
                    }
                    size++;
                    toY = (y + size > this.rowCount) ? this.rowCount : y + size - 1;
                    fromY = (y-size<0)?0:fromY;
                    toX = (x + size > this.colCount) ? this.colCount : x + size - 1;
                    fromX = (x-size<0)?0:fromX;
                }
            }
            //Console.WriteLine("-----------------------");
            //for (int i = 0; i < dArr.GetLength(0); i++)
            //{
               // Console.WriteLine(dArr[i]);
            //}
            //Console.WriteLine("-----------------------");
            if (found > count)
            {
                Array.Sort(dArr, cArr);
            }
            Array.Resize(ref cArr, count);
            Array.Resize(ref dArr, count);
            double rCumulated=0;
            double gCumulated=0;
            double bCumulated=0;
            double invDistance2Cumulated=0; // 1/d^2
            Color retVal;
            for (int i = 0; i < dArr.GetLength(0); i++)
            {
                rCumulated += cArr[i].R / dArr[i];
                gCumulated += cArr[i].G / dArr[i];
                bCumulated += cArr[i].B / dArr[i];
                invDistance2Cumulated += 1 / dArr[i];
                //Console.WriteLine(dArr[i]);
            }
            retVal = Color.FromArgb( (int)(rCumulated / invDistance2Cumulated)
                                   , (int)(gCumulated / invDistance2Cumulated)
                                   , (int)(bCumulated / invDistance2Cumulated)
                                   );
            //Console.WriteLine("-----------------------");
            //Console.WriteLine(retVal.ToString());
            //Console.ReadKey();
            return (retVal);
        }
    }
}
